#include "pid_float.h"
#include <stdio.h>
#include <stdbool.h>


/**
 * @brief  普通PID模式（无自适应）
 */
int pid_float_main(void)
{
    // 1. 定义float格式PID句柄
    WHT_PID_Float_Ctrl_t WHT_PID_Float_Ctrl;
    
    // 2. 初始化PID
    float kp = 0.5f;          // 比例系数
    float ki = 0.1f;          // 积分系数
    float kd = 0.3f;          // 微分系数
    float out_min = -0.8f;    // 输出最小值
    float out_max = 0.8f;     // 输出最大值
    WHT_PID_Float.WHT_Init(&WHT_PID_Float_Ctrl, kp, ki, kd, out_min, out_max);
    
    // 3. 配置积分限幅、微分滤波
    WHT_PID_Float.WHT_Config_Integral_Limit(&WHT_PID_Float_Ctrl, true, -2.0f, 2.0f); // 积分范围[-2.0, 2.0]
    WHT_PID_Float.WHT_Config_Derivative_Filter(&WHT_PID_Float_Ctrl, true, 0.9f);      // 滤波系数0.9
    
    // 4. 设置目标值（例：目标值0.5）
    WHT_PID_Float.WHT_Set_Setpoint(&WHT_PID_Float_Ctrl, 0.5f);
    
    // 5. 主循环：采集反馈→计算输出→控制执行器
    while(1)
    {
        // 模拟采集反馈值（例：反馈0.5678）
        float feedback = 0.5678f;
        
        // 计算PID输出
        float output = WHT_PID_Float.WHT_Compute(&WHT_PID_Float_Ctrl, feedback);
        
        // 打印输出
        printf("PID Out: %.4f\r\n", output);
        
        // 控制执行器（output为float，直接传入M4硬件控制函数）
        // 例：Set_PWM_Duty(output); 或 Set_DAC_Value(output);
        
        delay_ms(20); // 20ms周期，根据需求调整
    }
}

/**
 * @brief  自适应PID模式
 */
int pid_float_auto_main(void)
{
    // 1. 定义float格式PID句柄
    WHT_PID_Float_Ctrl_t WHT_PID_Float_Ctrl;
    
    // 2. 初始化PID
    float kp = 0.5f;
    float ki = 0.1f;
    float kd = 0.3f;
    float out_min = -0.8f;
    float out_max = 0.8f;
    WHT_PID_Float.WHT_Init(&WHT_PID_Float_Ctrl, kp, ki, kd, out_min, out_max);
    
    // 3. 配置积分限幅、微分滤波
    WHT_PID_Float.WHT_Config_Integral_Limit(&WHT_PID_Float_Ctrl, true, -2.0f, 2.0f);
    WHT_PID_Float.WHT_Config_Derivative_Filter(&WHT_PID_Float_Ctrl, true, 0.9f);
    
    // 4. 配置自适应调参
    WHT_PID_Float.WHT_Config_Adaptive(&WHT_PID_Float_Ctrl, true,
                                      0.2f, 0.8f,  // kp范围：0.2~0.8
                                      0.05f, 0.2f, // ki范围：0.05~0.2
                                      0.1f, 0.5f,  // kd范围：0.1~0.5
                                      0.125f, 0.0625f, 0.03125f); // 误差阈值：大=0.125、中=0.0625、变化率=0.03125
    
    // 5. 设置目标值
    WHT_PID_Float.WHT_Set_Setpoint(&WHT_PID_Float_Ctrl, 0.5f);
    
    // 6. 主循环：自适应调参+PID控制
    while(1)
    {
        // 模拟采集反馈值（例：反馈0.5678）
        float feedback = 0.5678f;
        
        // 计算PID输出（自动执行自适应调参）
        float output = WHT_PID_Float.WHT_Compute(&WHT_PID_Float_Ctrl, feedback);
        
        // 打印输出和当前kp/ki/kd（查看自适应效果）
        printf("PID Out: %.4f | kp: %.4f, ki: %.4f, kd: %.4f\r\n", output, WHT_PID_Float_Ctrl.kp, WHT_PID_Float_Ctrl.ki, WHT_PID_Float_Ctrl.kd);
        
        // 控制执行器（output直接用于M4硬件）
        // 例：Set_Motor_Speed(output);
        
        delay_ms(20);
    }
}
